clc
clear
close all;

%射击门
fire_dyt_pitch = 4.95;
fire_dyt_yaw = -2.27;
fire_error_max = 0.3;%射击门阈值，度,暂时改为0.3度

D2R = pi/180;
fd = fopen("file_4.txt");%%输入文件名称
fseek(fd,0,"eof");
fsize = ftell(fd);
fseek(fd,0,"bof");
num_line = fsize/128;   %128字节每个数据
dataty = zeros(num_line,128);
for i=1:1:(num_line)
    dataty(i,:)=fread(fd,128,"uint8");
end
fclose(fd);
size(dataty)
% disp(dataty);
row_num = find((dataty(:,5) ~=235) | (dataty(:,6) ~=22));   %删除帧头错误的数据
dataty(row_num, :) = [];
size(dataty)
% 时间戳数据
time=dataty(:,1)*16777216+dataty(:,2)*65536+dataty(:,3)*256+dataty(:,4);% 单位1表示10ms；
time = time * 0.01; % 单位为s;

%俯仰框架角
dyt_pitch_frame=(dataty(:,70)*256+dataty(:,69));
dyt_pitch_frame=double(typecast(uint16(dyt_pitch_frame),'int16'))*0.01;
% 方位框架角
dyt_yaw_frame=(dataty(:,72)*256+dataty(:,71));
dyt_yaw_frame=double(typecast(uint16(dyt_yaw_frame),'int16'))*0.01;
%目标脱靶量x
dyt_off_target_x = (dataty(:,64)*256+dataty(:,63));
dyt_off_target_x=double(typecast(uint16(dyt_off_target_x),'int16'))*0.05;
%目标脱靶量Y：像素的52.9682倍
dyt_off_target_y = (dataty(:,66)*256+dataty(:,65));
dyt_off_target_y=double(typecast(uint16(dyt_off_target_y),'int16'))*0.05;
%FK飞行模式
fk_flymode = dataty(:,7);
%对地高度
AltitudeToG = (dataty(:,19)*256+dataty(:,18));
% display(typecast(uint16(FKdata.AltitudeToG),'int16'))
AltitudeToG=double(typecast(uint16(AltitudeToG),'int16'))*0.1;
%DM距离
target_distance = dataty(:,103) * 256 + dataty(:,102);
target_distance = double(typecast(uint16(target_distance),'int16'))*0.1;



row_not_hkdj = find(fk_flymode ~= 170);
time(row_not_hkdj) = [];
dyt_pitch_frame(row_not_hkdj) = [];
dyt_yaw_frame(row_not_hkdj) = [];
dyt_off_target_x(row_not_hkdj) = [];
dyt_off_target_y(row_not_hkdj) = [];
AltitudeToG(row_not_hkdj) = [];
target_distance(row_not_hkdj) = [];
% time = time(row_hkdj);
% dyt_pitch_frame = dyt_pitch_frame(row_hkdj);
% dyt_yaw_frame = dyt_yaw_frame(row_hkdj);
% dyt_off_target_x = dyt_off_target_x(row_hkdj);
% dyt_off_target_y = dyt_off_target_y(row_hkdj);

dyt_pitch = dyt_pitch_frame + dyt_off_target_y;
dyt_yaw = dyt_yaw_frame + dyt_off_target_x;
%满足射击门的行
row_pitch = find(abs(dyt_pitch-fire_dyt_pitch)<=fire_error_max);
row_yaw = find(abs(dyt_yaw-fire_dyt_yaw)<=fire_error_max);
row_fire = intersect(row_pitch, row_yaw);
% row_fire = intersect(row_fire, row_hkdj);
fire_time = time(row_fire);
fire_dyt_pitch_frame = dyt_pitch_frame(row_fire);
fire_dyt_off_target_y = dyt_off_target_y(row_fire);
fire_dyt_yaw_frame = dyt_yaw_frame(row_fire);
fire_dyt_off_target_x = dyt_off_target_x(row_fire);

fire_AltitudeToG = AltitudeToG(row_fire);
fire_target_distance = target_distance(row_fire);
close all;
% figure()
% plot(time, dyt_pitch_frame);hold on;
% plot(fire_time, fire_dyt_pitch_frame,'.','LineWidth',3);hold on;
% legend("DYT俯仰框架角","满足射击门的框架角点")
% grid on;
% 
% figure()
% plot(time, dyt_yaw_frame);hold on;
% plot(fire_time, fire_dyt_yaw_frame,'*');hold on;
% legend("DYT偏航框架角","满足射击门的框架角点")
% grid on;

figure()
plot(time, dyt_pitch_frame);hold on;
plot(fire_time, fire_dyt_pitch_frame,'.','Color','r','LineWidth',3);hold on;
plot(time, dyt_off_target_y);hold on;
plot(fire_time, fire_dyt_off_target_y,'.','Color','b','LineWidth',3);hold on;
legend("DYT俯仰框架角","满足射击门的框架角点","DYT脱靶量Y","满足射击门的脱靶量")
grid on;

figure()
plot(time, dyt_yaw_frame);hold on;
plot(fire_time, fire_dyt_yaw_frame,'.','Color','r','LineWidth',3);hold on;
plot(time, dyt_off_target_x);hold on;
plot(fire_time, fire_dyt_off_target_x,'.','Color','b','LineWidth',3);hold on;
legend("DYT偏航框架角","满足射击门的框架角点","DYT脱靶量X","满足射击门的脱靶量")
grid on;

% figure()
% plot(time, AltitudeToG);hold on;
% plot(fire_time, fire_AltitudeToG,'.','Color','r','LineWidth',3);hold on;
% legend("对地高度","满足射击门的对地高度")
% grid on;

% figure()
% plot(time, target_distance);hold on;
% plot(fire_time, fire_target_distance,'.','Color','r','LineWidth',3);hold on;
% legend("DM距离","满足射击门的DM距离")
% grid on;

figure()
plot(time, target_distance);hold on;
plot(fire_time, fire_target_distance,'.','Color','r','LineWidth',3);hold on;
plot(time, AltitudeToG);hold on;
plot(fire_time, fire_AltitudeToG,'.','Color','b','LineWidth',3);hold on;
legend("DM距离","满足射击门的DM距离","对地高度","满足射击门的对地高度")
grid on;
% close all;
figure();
no_fire_dt = fire_time(2:end)-fire_time(1:end-1);
plot(no_fire_dt);

